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ABSTRACT 


KinGBnatic redundaincy in robot majiipulators has been found to 
be an advantageous feature from application point of view. 
Redundant manipulators are versatile and offer a broad spectrum of 
applicability. They posses useful features like dexterity, ability 
to avoid obstacles etc.. Presence of redundancy, however, 
complicates the control problem which is a major issue of research 
in this area. 

This work addresses the problem of control of redundant 
manipulators in tracking a specified end-effector trajectory. The 
objective is to determine the joint torques required to track a 
desired trajectory. A case of a three-Unk planar manipulator has 
been considered for the present study. 

The problem has been formulated as a fixed time Optimal 
Tracking Problem with fixed initial states and free terminal 
states. The redundancy is resolved by minimizing an integral 
performance index which has been constructed using the weighted 
norms of the tracking error and the joint torques. The dynamics of 
the manipulator has been used as the equations of constraint. The 
optimality conditions, as obtained from the Variational Principle, 
are used, which leads to a Non-Linear Two Point Boundary Value 
Problem (NLTPBVP). An iterative numerical algorithm has been used 
to solve the NLTPBVP. 

The efficacy of the scheme has been investigated by 
considering different performance criteria. The results of the 
simulations for various examples have been presented and discussed. 



CHAPTER 1 


INTRODUCTION 


1.1 INTRODUCTION 

One of the fundamental requirements of a general purpog^ 
manipulator is versatility in operation. This in turn imph^g 
dexterity and a broad spectrum of applicability of 
manipulator. In other words the manipulator should be able to 
perform a variety of tasks. Introdution of kinematic redundancy to 
a manipulator has been found to be a very useful solution to iQ^et 
such requirements. This follows from a close observation of the 
human arm which has a high degree of dexterity and verssatlUty 
due to the redundancy it possesses. Kinematic redundancy not only 
increases the versatility of the manipulator arm but also renders 
it with certain useful properties which are discussed in this 
chapter. 

A robot manipulator is said to posses kinematic redundancy 
if it has more degrees-of -freedom than is required for performing 
a specified task. Redundancy, in a more general sense, is defined 
relative to a particular task. For example. In the three 
dimensional catrtesian space, the general motion of the 
end-effector is specified by six independent quantities, three foj. 
positioning and three for orienting. A spatial manipulator havin 

9 
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more than six degrees-of -freedom is said to posses kinematic 
redundancy. Likewise, for a three degrees-of-freedom planar 
manipulator, the position and orientation of the end-effector is 
specified completely by three independent coordinates. If on the 
other hand, the task of the manipulator is specified in terms of 
the position of the end-effector alone, without any reference to 
its orientation, then, relative to such a task , the manipulator 
is redundant. 

There are many interesting features of redundant 
manipulators. Detailed analyses presented in the next chapter 
indicate that infinite sets of joint trajectories can produce the 
same end-effector trajectory. Redundancy thus offers a tremendous 
flexibility in selection of a joint trajectory for a specified 
end-effector trajectory which can be utilized tactfully to satisfy 
a number of other requirements. This property brings in 
versatility in operation which is advantageous in a numerous 
applications. Some of the possible advantages of redundant 
manipulators are 

1. Potentlal[,fe<^ avoid singular configurations. 

(f 

2. Ability to avoid obstacles. 

3. Ability to negotiate curvatures to reach to a location 
which are otherwise inaccessible. ^ I 

« i 

4. Ability to avoid structural limitations. { / 

5. Scope for configuration or posture control. 
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Moreover, redundant manipulators are more reliable in the sense 
that they can perform certain tasks even in case of joint 
failures. This is particularly useful in case of deep sea and 
space applications. 

However this richness in the choice of joint trajectories 
complicates the control problem considerably which has motivated 
many researchers to propose various algorithms to resolve the 
problem. 


1.2 LITERATURE REVIEW 

The significance of redundancy was first pointed out by 
Whitney [1969, 1972]. He proposed the principle of resolved motion 
rate control in which the end-effector velocities are related to 
the joint velocities using the Jacobian matriK. The required 
instantantaneous joint velocities for a given end-effector 
velocity can be determined by inverting the Jacobian matrix. For 
rectangular Jacobian matrix as in the case of redundant 
mcuiipulators, Whitney proposed to minimize a quadratic function of 
the instantaneous joint velocities at every knot point on a given 
Cartesian space trajectory. The solution turned out to be the 
weighted pseudo-inverse of the full rank Jacobian matrix of the 
manipulator. 

Liegeois [1977] was the first to discuss the active 
utilization of redundancy of robot manipulators. He used the 
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generalized inverse of the Jacobian matrix. He proposed the 
minimization of a scalar function by projecting it's gradient onto 
the null space of the Jacobian matrix. Using numerical simulation, 
he demonstrated one such potential function to keep the Joint 
angles within physical limits. 

A comprehensive review of the pseudo-inverse approach to 
control of redundant manipulators had been presented by Klein and 
Huang [1983]. They had pointed out some problems such as drift in 
the joint space trajectory with pseudo-inverse control. Drift 
results in non-conservative joint trajectories i.e., closed 
trajectories in the cartesian space does not result in closed 
joint space trajectories. They had demonstrated this effect using 
a four degrees of freedom planar mcinipulator tracking a square 
end-effector trajectory. 

Baillieul [1985] proposed an Extended Jacobian Method for 
solving the redundancy problem which optimizes a scalar function. 
His method requires the initiad configuration of the robot to be 
such that the scalar function assumes an optimal value at the 
initial point. He also pointed out that using the pseudo-inverse 
control, the global optimal solution is not guaranteed. 

Certain special configurations of a manipulator where the 
rank of the Jacobian matrix is less than the full rank are termed 
as Singular configurations. In the case of redundant manipulators, 
the redundancy can be utilized to avoid the singularities. 
Yoshikawa [1985(a), 1985(b}, 19903 defined the manipulability 
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measure which quantitatively describes the distance of a 
manipulator configuration from a singular configuration. He 
proposed to use the generalized inverse of the Jacobian matrix in 
such a way that a scalar function, defined as the manipulability 
measure, is maximized as the manipulator moves, thereby avoiding 
singular configurations. 

Local minimization of actuator torques in a least square 
sense by resolving redundancy at joint acceleration level was 
proposed by HoUerbach and Suh [1985]. Their scheme exhibited 
unexpected instability for relatively long cartesian space 
trajectories. 

Maciejewski and Klein [1985] presented an approach to 
determine the required joint angle rates for a manipulator under 
constraints of multiple goals. The primary goal was to track a 
specified end-effector trajectory and the secondary goal, to avoid 
obstacles. The primary goal was satisfied exactly by the 
particular solution of the pseudo-inverse of the Jacobian matrix. 
The homogeneous component of the solution maximized the distance 
of the manipulator from the obstacles. The implemention was fast 
enough to work efficiently in a dynamically changing environment. 
This was similar to the task priority based approach of Hanafusa 
et al [1987]. 

The concept of task priority was introduced by Hanafusa et 
al [1987] into general redundancy control problem. It requires the 
desired task to be broken up into sub-tasks of different 
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priorities and the sub-tasks with lower priorities are realised 
using the redundancies not committed to satisfy the sub-tasks with 
higher priorities. 

Kazerounian and Nedungadi [1987] developed cui algorithm to 
locally minimize the driving torques by resolving redundancy at 
the joint velocity level. This resulted in high joint acceleration 
and hence high torques leading to instability. They concluded that 
there has to be a trade-off between joint driving force 
optimization and algorithm stability. 

Dubey and Luh [1987] suggested the Manipulator Velocity 
Ratio (MVR) as a measure of manipulabiJity. It is defined as the 
ratio of the norm of end-effector velocity and norm of joint 
velocity. For redundant manipulators, MVR is not unique for the 
simple reason that given the end-effector velocity, the joint 
velocity is not unique- They had proposed a control algorithm 
which progressively maximized the minor axis of the Manipulator 
Velocity Ratio Ellipsoid (MVRE) which represents the minimum value 
of the MVR, as the manipulator moves. This ensured uniform 
manipulability in aU directions and hence avoided singular 
configurations. 

Anderson and Angeles [1989] used a constrained non-linear 
least-square minimization approach in which a positive definite 
performance index was minimized subjected to the kinematic 
constraint equations. The resulting non-linear problem was solved 
as a sequence of linear quadratic problems. The algorithm resulted 
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in conservative joint motions. 

Dubey et al [1988,1991] proposed a gradient projection 
optimization scheme in the framework of the resolved motion rate 
control. Their scheme did not require the calculation of the 
pseudo-inverse of the Jacobian matrix but otherwise similar to 
that used by Liegeois [1977]. 

In cdl the research works cited above, the redundancy 
resolution was based upon a local optimization of some performance 
criterion or objective function while carrying out a task. Most of 
them proposed to use the generalized Inverse of the full raink 
Jacobian matrix, the range space of which is used to track a 
specified cartesian space trajectory and the null space to carry 
out the secondary sub-task such as avoidance of collision, 
obstacles, singularity and joint limits. Though computationally 
fast cuid wen suited for on-line implementation, these schemes 
have drawbacks some of which are listed below. 

1. They may not result in the "best" joint space trajectory 
for complete manipxilator motion. In other words, they do not 
guarantee a globally optimal solution. 

2- Closed trajectories in cartesian space may not result in 
closed trajectories in joint space, i.e. non-conservative joint 
space trajectories may result, which is undesirable in case of 
repetitive and cyclic tasks. 

3. Singularity avoidance in a strict sense may not be 
achieved by locally maximizing the manlpulability measure 
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(Baillieul [1985]). Moreover, in using certain methods (Yoshikawa 
[1985(a), 1985(b), 1990]), the manipulability in certain directions 

may reduce drastically. 

4. Local optim iza tion schemes may result in some 
instabilities as has been observed, for example, by HoUerbach and 
Suh [1985, 1987(a)] and Kazerounian and Nedungadi [1987]. 

There is yet another class of research which addresses the 
redundancy resolution problem and proposes to optimize a 
performance measure in a global sense, that is, over the entire 
trajectory of the end-effector. 

Whitney [1971] suggested minimization of a global criterion 
which was in the form of minimization of the kinetic energy of the 
manipulator over the whole trajectory. He did not propose any 
computational method. 

Nakamura and Hanafusa [1987] solved the global optimal 
redundancy resolution problem by using the Pontryagin's Maximum 
Principle. The problem is reduced to a two point boundary value 
problem (TPBVP) and the solution is obtained by a minimal value 
search. 

HoUerbach and Suh [1987(b)] investigated and compared the 
local and global optimization schemes for minimizing the torque 
loading at the joints of a redundant manipulator in a least square 
sense by resolving the redundancy at joint acceleration level. 

They observed that local optimization methods may show an 
unexpected instability for relatively long trajectories for which 
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global optimization methods always converged. 

Kazerounian and Wang [1988] used the Calculus of Variation 
to solve the problem of redundancy resolution by minimizing the 
square of the norm of joint velocities over the whole trajectory. 
They also proved that local minimization of joint accelerations 
result in a global minimization of the joint velocities. 

Sheraji [1989] proposed a configuration control scheme for 
redundant manipulators over the entire end-effector motion. In his 
method some kinematic functions in the form of constraints are 
introduced to reflect the additional task to be performed due to 
redundancy. The scheme ensured cyclic motion of the manipulator. 
Based on the Model Reference Adaptive Control (MRAC) approach, the 
adgorithm neither requires the complex dynamic model of the 
manipulator nor the Jacobian matrix inversion. It was found to be 
computationally fast and suitable for real time implementation. 

Martin et al [1989] solved the redundancy control problem by 
minimizing an integral performance index subject to path 
constraints in the cartesian space using the Lagrangian approach. 
The algorithm resulted in conservative trajectories in the joint 
space for cyclic paths in the catesian space. 

More recently Gupta and Luh [1991] proposed a closed loop 
state feedback motion control scheme for redundant manipulators 
based on Hamilton-Jacobi-Bellman equation. They defined a 
quadratic performance criterion which was minimized over the 
entire trajectory with redundancy being resolved at the joint 
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acceleration level. The algorithm used the linearized equations of 
dynamics of the manipulator. 

A unified approach to the Inverse kinematics solution for 
redundant manipulators was put forward by Won et al [1993]. They 
proposed to resolve the redundancy at the joint velocity level 
irrespective of the performance index chosen. They had 
demonstrated the validity of their approach by applying it to a 
three link planar manipulator for various types of performance 
indices. 

1.3 OBJECTIVE AND SCOPE OF THE PRESENT WORK 

This work adresses the the problem of tracking a specified 
end-effector trajectory by a redundant manipuator. The objective 
of the problem is to determine the required joint torques, 
considering the dynamics of the manipulator, at all the points 
over the trajectory in order to track the specified trajectory by 
the end-effector. 

At the most basic level researchers have proposed redundancy 
resolution methods involving optimized solution of the inverse 
kinematics problem. All the local and most of the global 
optimization schemes require the generalized inverse of the full 
rank Jacobian matrix. Near the singular configurations the 
Jacobian matrix inversion results in undesirably high joint rates. 
Moreover dynamics of the manipulator has not been considered in 
the optimization process by most of the researchers. This might 
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lead to very high joint torques near the singular configurations. 

This work attempts to solve the redundancy control problem 
following the optimal control approach. The primary objective has 
been to solve the global optimal control problem and at the same 
time eliminate the need to resolve redundancy at velocity or 
acceleration level. This obviates the inversion of the Jacobian 
matrix thereby avoiding the instability problem due to 
singularities. The equations of dynamics of the manipulator have 
been taken into consideration in the optimization process by using 
them as equations of constraint. 

The problem formulation, as has been described later, is 
very similar to the formulation in the recently reported work by 
Gupta and Luh [1991] although the solution procedure is different. 
The present formulation, based on the Linear Quadratic Problem, is 
known as the Optimal Tracking Problem. An integral performance 
index has been defined which involves the tracking error and the 
joint torques over the entire range of motion of the manipulator. 
The equations of motion of the manipulator have been considered as 
the constraint equations. The problem is solved using the 
Variational Principle which results in a Non-Linear Two Point 
Boundary Value Problem (NLTPBVP). Analytical solution of such a 
problem is very difficult and calls for numerical solution. 

A family of ailgorithms for solving continuous-time 
unconstrained non-linear optimal control problems have been 
proposed by Nedeljkovic [1981]. One of the algorithms has been 
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found suitable for solving the problem in hand. 

The algorithm has been used to study some aspects of 
redundancy control through optimization. Different forms of 
performance indices have been constructed, the results analysed 
and presented. Through the various simulations, a comprehensive 
understanding of the global optimization approach to redundancy 
resolution has been sought. Further some of the results have been 
compared with the solutions obtained using the pseudo-inverse of 
the Jacobian matrix. 

The scope of the present work has been limited to the fixed 
time optimal control problem. The study has been done for a three 
degrees-of-freedom planar redundant manipulator and the algorithm 
has been tested through numerical simulations. 


1.4 ORGANIZATION OF THE THESIS 

This thesis has been organized into five chapters. The 
second chapter deals with the analysis of redundant raainipulators 
from the kinematics view-point. The optimal control formulation of 
the trajectory tracking problem and an iterative numerical 



numerical results for various examples considered have been 
presented and discussed to highlight some of the aspects of the 
trajectory tracking problem. The final chapter concludes the 
present work and suggests scope of future work. 



CHAPTER 2 


KINEMATICS OF REDUNDANT MANIPULATORS 

2.1 INTRODUCTION 

Redundant mainipulators have a number of advantages over the 
non-redundant ones. Some of the interesting and advantageous 
features have been described in the previous chapter. Such special 
features can be explained on the basis of the kinematics of 
redundant manipulators. This chapter deals with the basic theory 
of redundant mainipulators from the. kinematics view-point. The 
problem of forward and inverse kinematics of redundant 
manipulators has been discussed in the second section. Some of the 
implications of redundancy have been pointed out. In this work, a 
three degrees-of -freedom planar manipulator has been considered 
for the study and the equations of kinematics and the motion rate 
equations of the manipulator have been presented in the third 
section of the chapter. 

2.2 FORWARD AND INVERSE KINEMATICS RELATIONS 

The problem of forward kinematics in robotics is concerned 
with determination of the position and orientation of the end 
effector of the manipulator in terms of the joint variables. On 
the contrary, the inverse kinematics problem is concerned with 
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solving the joint variables when the position and orientation of 
the end-effector aire specified. 

2.2.1 The Forward Kinematics Equations 

Consider an m degree-of-freedom robot, as shown in Fig. 2.1, 
having m joint variables ais q . . . . q . These joint variables 

12 m 

can be represented in vector notation as 

q = tq, q, • • • q 

12 m 

where, 

q^ represents the joint variable corresponding to the i*'*’ 
joint. 

Let the position cund orientation of the end-effector coordinate 
frame, O^X Y^Z^, be specified completely by p independent 
quantities expressed in the World Coordinate System (WCS). Let 
these variables be represented in the vector notation as 

y = [y, • • • yp3^ 

where, 

y. is along the coordinate direction. 

The value of p represents the minimum number of independent 
quantities reqioired to completely specify a rigid body in the WCS. 
For example, for a general two-dimensional planar motion, p = 3 as 
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X. 



G. 2.1 REPRESENTATION OF A GENERAL m DEGREE-OF-FREEDOM MANIPULATOR 
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two translational and one rotational coordinates completely 
specify the rigid body in a plane. Likewise, for a general 
three-dimensional motion, p = 6, since three position and three 
orientation coordinates completely specify the rigid body as in 
the case of in Fig. 2.1. Each of these p quantities can 

be expressed cis functions of the joint variables. These functions, 
which are in general non-linear, can be derived purely from 
geometry and represented in the form 


= q^- . 







■ • • 

(2.1(a)) 


or in a more compact form as 


y = f(q) (Z.Kb)) 

where , 

f( • ) - [f , ( •) f.,( • ) . . . f ( O]^. 

2.2.2 The Inverse Kinematics Equations 

Determination of the joint variables required to attain a 
specified position and orientation of the end-effector is the 
problem of inverse kinematics. In other words, given the 
end-effector position vector y, the problem is to determine the 
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joint variable vector q by solving (2.1). The complicacy of the 
problem is primarily due to the non-linearities involved in the 
kinematics relations described above. Another point which needs to 
be investigated is the solvability in terms of the number of 
equations and number of unknowns involved. 

Consider the system of equations (2.1). In certain 
situations it may so happen that only n of the p quantities at the 
end-effector are of significance. Then depending on the values of 
m, p and n, the manipulator may be termed as redundant or 
non-redundant as discussed below. 

(i) Non-Redundant Case i If, m = n = p, then there are m 

equations in m unknowns which may be solved for a unique or at 
most a finite set of solutions of q and the robot is said to be 
non-redundant. f / 

(ii) Redundant Case : If, (/n = p, n^< p) or (m > p, n = p), 

the number of unknowns involved are nwre than the number of 

equations. An infinite number of solution sets of q are possible 
and the robot is said to posses (m - n) or (m - p) redundant 

degrees-of-freedom as the case might be. When (m = p, n < p), the 

presence of redundancy is decided by the task the manipulator 
performs, while in the situation when (m > p, n = p), the 
mamipulator is redundant irrespective of the task specified. 

An end-effector trajectory can be considered as a finite set 
of points in an n-dimensional real space while a joint trajectory 
can be considered as finite set of points in an m-dimensional real 
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space. These points are known as knot points. For redundant 
manipulators, at every knot point in the cartesian space, there 
exists infinite solutions of the joint variables ais the number of 
unknowns are more than the number of equations. Thus for a 
redundant manipulator, infinitely many sets of trajectories can 
exist in the joint space which results in the same end-effector 
trajectory. This is the most important feature of a redundant 
manipulator and to exploit it meaningfully has been the subject of 
research in this area. 

2.2.3 Inverse Kinematics of Redundant Manipulators Using Motion 
Rate Equations 

In order to perform a task, a definite solution of the joint 
variables of the manipulator has to be chosen out of the infinite 
sets of solutions as mentioned above. The choice of the inverse 
solution is made either by introduction of secondary tasks to be 
carried out or by imposing a performance criteria to be satisfied. 
For example, minimization of the weighted norm of the joint 
velocities is a possible guideline for the selection of a joint 
space trajectory as suggested by Whitney [1969, 1972]. 

The motion rate equations of the manipulator are written by 
diff erentiating (2.1) once with respect to time as 

y' = J q' (2.2) 


where, 
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y' = ^ is a vector of size n x 1, 
q' = ^ is a vector of size m x i and 

J “ dq ^ Jacobian matrix of the manipulator of 

size n y- m and n < m. 

The optimization problem is then formulated as 

Minimize (q'^ W q' ) 

subject to 

x' = J q', 

where, 

W is the weight matrix on the joint velocities- 

This is a simple constrained optimization problem. The solution 
obtained from this minimization procedure is represented as q^ and 
is of the form (see Appendix A) 

q' = J* x' (2.3(a)) 

where, 

J* = w'^J^ (J W'^j’^) is known as the weighted pseudo-inverse 
of J. 

The solution for joint velocity vector q^ obtained from 
(2.3(a)) is known as the range space solution of J- The Jacobian, 
being a recan gular matrix, has yet another solution, the null 
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space solutionas given by (Yoshikawa [1985]) 

= (I - J^J) k (2.3(b)) 

where, 

is the null space solution of J, 

I is an m X ;jj identity matrix and 

ir is an flj X 1 arbitrary non-zero vector. 

Hence the totcil solution is given by 

q' = x' + (I - j’^J) k (2.4) 

The joint velocity solution q', cis given by (2.3(a)), 
produces the desired end-effector motion, while, q^, as obtained 
from (2.3(b)), only contributes to change the cirm configuration 
which might be required for secondary teisks to be performed. There 
are infinite solutions of q^ depending on the form of k chosen in 
(2.3(b)). The choice of Jr is made on the basis of the secondary 
task to be carried out. 

Since the Jacobian is a function of the joint variables q, 
there may exist one or more q such that the Jacobian is not of 
full rank, i.e.. Rank (J) < n. The configurations corresponding to 
such values of the joint variables are termed as the singular 
configurations and the corresponding end-effector positions are 
the singular points of the workspace. If the Jacobian matrix is 
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not of full rank, i.e. at the singular points, the solution of q' 
as given by (2.4) does not exist. In the neighbourhood of the 
singular points, the solution results in impracticably high joint 
velocities. 

2.3 CASE OF A PLANAR THREE-LINK REDUNDANT MANIPULATOR 

In this section the special case of a planar three link 
manipulator with revolute joints. The equations of kinematics and 
the motion rate equations are also derived. 

2.3.1 Forward Kinematic Relations 

Consider a three-link planar manipulator with revolute 
joints, as shown in Fig. 3.1(a). The joint variable vector q, in 
this case, is defined as 


q = [ 0 0 e f 

^12 3 

where, 

0. is the joint variable. 

The end-effector position is specified by y^ and y^, which 
represent the position coordinates along the and directions 
of the WCS as was shown in Fig. 3.1(a). The orientation of the 
end-effector is not considered as a task variable and thus the 
end-effector is specified by only two variables namely y^ and y^ 
which can be expressed in terms of the joint variables, using 



Chapter 2 Kinematics of Kedundant Manipulators 


22 


geometry, as 


= 1 , 

cos 

0 

■f 

1 

COS 

0 

+ 

1 COS 0 

t 


1 


2 


12 


3 123 


sin 

e 


1 

sin 

0 

+ 

1 sin 0 

1 


1 


2 


12 


3 123 


where, 

1 ^, Ig* I 3 aire the link lengths and 

0, = 0 + + . . . + e . 

1 - . . n 1 2 n 


(2.5(a)) 

(2.5(b)) 


The end-effector of this manipulator is required to trace a 
planar trajectory. At every point of the trajectory, the 
coordinate values of the point O^is specified with respect to tlie 
world coordinate system in terms of y^ and y^ without any 
reference to the orientation of the end-effector coordinate axes. 
The end-effector position vector y is then represented as 


y = [y, y^l'' 


In order to determine the joint motions that resiilt in the 
specified end-effector trajectory, the equations (2.5(a)) and 
(2.5(b)) have to be solved simultaneously. There are only two 
equations involving three unknowns namely 0 ^, and 0 ^ and thus 
there is one redundant degree-of-freedom in the manipulator 
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2.3.2 Motion Rate Equations 

The motion rate equations of the manipulator can be written 
by differentiating the equations {2.5(a)) and (2.5(b)) as 


y'., - ~ sin0, - 0' sin0,^ 

1 11 1 212 12 

y' = 1 0' COS0 + 1 0' cos0,^ + 

2 11 1 212 12 


- 1 0' sin0 

123 ►-»***• 123 


1 0' COS0 

3 123 123 


or in a more compact form as 


y' = J q' 


where, 


{2.6(a)) 


(2.6(b)) 


J = 



1^COS0^ + l^cosJ^^'*' igCO-SO 

-l^sine^^- l3Sine,33 

1 COS0 1^COS0,^^ 

2 12 3 123 


1 COS0 

3 123 


123 


T 


(2.7) 

is the Jacobian matrix of the manipulator , 
y' = [y^ is the end-effector velocity vector and 

q' = [0^ 0^ 0^]^ is the joint rate vector. 



CHAPTER 3 


OPTIMAL CONTROL FOEMULATICM FOR PATH TRACKING 

3.1 INTRODUCTION 

This chapter deals with the trajectory tracking problem 
considering the dynamic model of the manipulator. In the second 
section, the equations of dynamics for the three-link planar 
manipulator, considered in this study, have been presented. The 
third section addresses some issues related to the trajectory 
tracking problem. In the fourth section, the trajectory tracking 
problem has been posed as a fixed time Optimal Tracking Problem 
with fixed initial states and free terminal states for the 
three-link planar redundant manipulator. An integral performance 
index has been defined which is required to be minimized over the 
trajectory, subject to constraints in the form of the equations of 
motion of the manipulator. Using the Variational Principle, the 
problem reduces to a non-linear TPBVP which is very difficult 
solve in closed form. An iterative numerical scheme used for the 
solution purpose hais been presented in the last section. 

3.2 THE DYNAMICS MODEL 

In this section the equations of motion for the three Unk 
planar manipulator with revolute joints, shown in the Fig- 
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have been presented. The i*'*' coordinate frame, O., is fixed to the 

1 

i*'*' link and positioned at the (i+1)*'*' joint with Z. along the 
axis of the joint as shown in Fig. 3.1(a). The manipulator is 
considered to be in a horizontal plane and hence the gravity 
forces cire not considered. The equations of motion have been 
derived using the Lagrange-Euler formulation. 

3.2.1 Lagrange-Euler Formulation 

The Lagrange-Euler equation of motion is of the form (Fu, 
Gonzalez eind Lee [1987]) 



where, 

£ = K - P is the Lagrangian of the system, 

K, P represent the total kinetic energy and potential 
energy of the system respectively, 
q^ is the i*'*' generalized coordinate describing the 
system state and 

is the i*'*' generalized force along the respective 
coordinate direction. 

Consider a differential mass dm^ on the i*'*‘ link as shown in 
Fig. 3.1(b). The kinetic energy, dK^,, due to this mass can be 
expressed as 
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dK 


where. 


. =■ \ Trace fO 

' ^ V) dm. 


(3.2) 


V. is the velocity of tha , 
* 


in the WCS. 


^tential mass dm. expressed 


The velocity vector is expressed 




V. a: d 


where. 




(3.3) 


r. is the position vector 
the differential mass di^ 
in Fig. 3.1(b). 


it 


^homogeneous coordinates, of 


* ^^Pressed in the WCS as shown 


Let r. be the position voct. 


Of. 


of the differential mass dm,, 

1 

frame as 


it homogeneous coordinates. 


®®S6d in it's own coordinate 


where, 

X. 


^ r 

r. = [ X 


i 0 


0 1 


(3.4) 


is the coordinate 
shown in Fig. 3.1(b). 




^ along the X. direction as 


0 1 

The position vector r^ can be 
the homogeneous transformation 


tenv 

^®tnted in terms of 



using 
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0 


r 


i 




(3.5) 


where, 

^ is the honiogenecns transformation 

matrix relating the i*'*’ and the world coordinate 
frames. 


For the planar manipulator in Fig. 3.1 (a), the homogeneous 
transformation matrix relating the i*'*’ coordinate frame and the 
(i-l)''*’ coordinate frame is given by 


i 



cos -sin 

sin cos 0^ 

0 0 

0 0 


0 Ij cos 0^ 

0 1^ sin 

1 0 

0 1 


where, 

1. is the link length of the i^'' hnk and 
0, is the i*'*' joint variable. 


Now from (3.3) and (3.5) one obtains 
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i 

z 




Vue' 
Z-. ‘ j j 




(3.6) 


where. 


q' is the time derivative of the j joint variable and 




Substituting the expression for ^v^ from (3.6) in (3.2) and 
simplifying yields 


dK = -x Trace 

i 2 


E E 'Jip 


e' d' 

ir P r 


Hence the totad kinetic energy of the link i is obtained by 


K 


i 



or. 


K. ~ Trace 
1 2 


= - Trace 
2 


EE 

pal r 5= 

EE 

n* 1 r =s ■ 


U. ( f *r. ‘r^ dm.) 


U I 0' 0' 

i p i i r P ** 


6 ' 6 ' 
i r p r 


(3.7) 


where, 
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I 


1 



i T , 

dm 


i 


Substituting the expression for ^r^ from (3.4) yields 




Xj 0 0 x^ 

0 0 0 0 

0 0 0 0 


Xj 0 0 1 


dm 


Using the inertia tensor notation, the above integration reduces 
to 




0 

0 


m.p. 
1 1 


0 

0 

0 

0 


0 

0 

0 

0 


- 

0 

0 

m. 


where, 

p^ is the position of the center of mass of the link 
expressed in the f*’*' coordinate frame and 
I is the moment of inertia of the link i about the 

Z.-axis 


Using parallel axis theorem for the moment of inertia, I can be 

zZi 
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expressed as 


I 

zz . 


_ X . 2 

= I. + ra.p 

i I i 


where, 

Ij is the moment of inertia of link i about the centre of 
gravity. 

The total kinetic energy of the manipulator, with all the links 
taken together, can now be written using (3.7) as 


K 


= 1 ^ 




r 3 I i 


or. 


K = - Trace 


7 y y U. I, 8' 8' 

La La Lj. 1 ir p r 


isl p=l r=:t 


Expanding the summation, the expression for the total kinetic 
energy is obtained as 


K 


= - Trace f l 
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Substituting the expressions for various terms and simplifying one 
obtains 


+ - 21 ^P^ + 21 ,p^c^ + + I3 + m3( P3 - 2P3I3 

- 2l3p3=3 - 21,P3C33 + ‘3 + I3 t + 2I3I3C3 + 2:31,033 


^ + ( I 2 + 


“ 2 <P 2 - + 21/303 + 1 


+ I 3 + m^CPg - 2 I 3 P 3 + I 3 + ig + ZPgl^Cg + Pgl^C^g 

+ ^2 + i [ ^2 + "^3 ^p 2 - V2 + ^ 2 ^ 

+ 13 + m3 (p^ - 2p 1 + 1 + 1^) ] B'J + 


1 + ra (- 2p 1 + 1 + 1 1 + u c_) e: 0: 


3 2 3 3 1 23 


+ I3 + m3(p^ - 2P3I3 t I3I3C3 t 1^1 


I-, ~ 2 ra,p 1 + ml 
3 33 3 3 : 


] d'‘ 
3 ) 3 


0' 0' 
j 2 3 


where, 


P. = 1. - P.. 

1 I 1 


Since the manipulator is considered to be in the horizontal 
plane, the gravitational potential energy, P, is put to zero. 

Substituting the expressions of K and P in (3.1} and 
simplifying, the equations of motion of the three-link manipulator 
Eire expressed as 
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[^1 + ^2 + ^3 * '”3‘,'>3“=<®23> ^ 2m3l3P3COS(03l + m3P^ + 

3131,13003(63) + m3l,P3Cos(e3) + nl3l| + m3P^ + 3133!^ + 

■33131,003(63) + m3P3l,oos(63) + ii,3P3l, 003(633) + ni,pj ) 6 -; + 

Ug + Ig + 20131303003(63) + iii^p^ + 10,1^ + + m^l^l,cos(6^) 


3‘ 3 


3 2 


2‘ 2 


3 2 1 


+ 013631,003(63) + 013631,003(633)] 6" + [13 + 1036' t 


0136313000(63) + 013631,003(633)] 6 " + [013!, 




P3®;"®‘nl®23> + 

1/2 






'"3<'3'2«;33=‘''‘®3' 


[^3 ^3 + "'3*l''3“’=‘®23> + 20131363003(63) + IO3P3 + 

01,1.1,003(63) + 0131,63003(63) + 0I3I3 + 1O3P3 ] e; + [13 


3 12 


+ ^3 + 


+ ™3P3 + ^3^1 + "*2^2 ) ^2 


^3 ^ ^3^3 




,P 3 l 2 CO 8 ( 03 )) e; + [m3l,P30;''sin(023) + m 3 l 2 P 30 ; ^ 8 ^( 03 ) + 


+ ^ zK z'> + 


/ 2 


r «/2. 


c 0 
2 2 


;]■ 


[h + ™3liP3''°®<^23> + 2 m 3 l 3 P 3 Cos( 03 ) + m^pl ] 0 ; + [I 3 + 

m3l3P3COS(03) + tn3P^ ] 0" + [I3 + m3P^ ] 0^ + (">31/36; ^5111(033) 

+ ‘'3^3) = ''s 


where. 


0 = 0 + 0 + 

1. -n 1 2 
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m „ = m + m + . . . + m , 

1 . . n 1 Z n 


d0. 

e: = — ' , 

' dt 
d^e 

d" = — , 

‘ dt^ 

- Pj, 


Cj is the coefficient of viscous friction in the i^^ joint. 

The frictional forces at the joints have been included in 
the equations of motion above as generalized joint forces and 
represented by 


where, 

is the viscous frictional torque on the t *'*' joint. 

The above equations of dynamics can be represented in a 
general form as 


[M(q)]q" + C{q,q') + F(q' ) = u (3.8) 


where. 
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^ ~ ^ joint variable vector, 

q ~ t ^3 joint velocity vector, 

q ~ ^3 ^ joint acceleration vector, 

u ~ t ^2 ^3 ^ joint torque vector, 

M(q) is the inertia matrix of size 3x3, 

C(q,q ) is the Coriolis and centrepetal force vector of size 
3x1 and 

F(q') if the friction force vector of size 3x1. 

The detailed expressions for the matrix elements are presented in 
Appendix B. 

3.3 TRAJECTORY PLANNING PROBLEM 

The trajectory plauining problem, for a general manipulator, 
is concerned with determination of the joint torque vector u, 
when, the end-effector trajectory is specified. Here, the problem 
is discussed with reference to redundant manipulators. If, the 
time of travel, t^, is also specified, then, any of the following 
approaches may be adopted as discussed below. 

One approach is through motion rate control. The Jacobian of 
the manipulator is obtained from the motion rate equations which 
is inverted to obtain the joint rates at each knot point required 
to track the specified end-effector trajectory. In case of 
redundant manipulators the pseudo-inverse of the Jacobian is used 
as was discussed in Sec. 2.2. Once the joint velocities are 
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obtained, the joint acceleration and joint angle trajectories can 
be determined using finite difference methods. The inverse 
dynamics equations are then used to calculate the reqiiired joint 
torques. 

The motion rate control is a local optimization scheme in 
the sense that it resolves the redundamcy by minimizing the norm 
of the joint velocities locally at each knot point. There is 
another approach in which a global performance criterion in the 
form of an integral performance index is minimized. The problem 
Ccin be posed as an Optimal Tracking Problem based on the Linear 
Quadratic Problem. This approach has been discussed below in 
detail. 

3.4 TRAJECTORY TRACKING THROUGH OPTIMAL CONTROL FORMULATION 

3.4.1 Performance Criterion and the Tracking Problem Formulation 
In order to formulate the problem as an Optimal Tracking 
Problem, the stringency on the requirement of exactly tracking a 
specified end-effector trajectory is relaxed. An error vector 
function is defined which represents the deviation of the actual 
trajectory from the desired trajectory of the end-effector at 
every knot point as 


e = y - y^ 


where, 
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e represents the end-effector trajectory error 
y represents the generated end-effctor trajectory and 
y^ represents the desired trajectory- 


Now, a positive definite scalar function is defined in the 
form of weighted quadratic function of the error vector which is 
required to be minimized over the entire trajectory and this 
minimization criterion is the primary performance requirement. The 
above requirement of tracking a workspace trajectory provides only 
a kinematic constraint without considering the dynamics of the 
manipulator. So minimization of just the trajectory error may 
result in high torque requirement at the joints. In order to keep 
a bound on the joint torques, the performance index is modified by 
appending a weighted quadratic function of the torque vector. 
Hence, the performance index, J, for a fixed time problem is 
written in the following form. 


where. 


J 


2 

2 


t 


(e’^Q^e + u) dt 


0 


( 3 . 9 ) 


is a positive definite symmetric weighting matrix on the 
tracking error, 

R is a positive definite symmetric weighting matrix on the 


joint torques. 
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u is the joint torque vector and 
t^ is the specified final time. 


Secondary performance objectives such as global minimization 
of joint velocity norm or kinetic energy can also be specified. An 
additional requirement on achieveing a final state in terms of 
position and/or velocity can be imposed by putting a penalty on 
the error from the desired state at the final time. The 
augmented performance index can then be expressed as 


L 


-r A 1 ^ . A 

J = ~ZQ2 

2 f 2 


Z + U^R u) dt 


0 


where, 

2 = [e q']’’. 



Q = diag [Q^ and 

Qf = Q 


(3.10) 


The sub-matrix of Q decides the secondary performance 
criterion which has been discussed in chapter four. 

The joint torque vector, u, is related to the joint 
variables and their derivatives through the equations of dynamics 
of the manipulator as given by (3.8). For the solution purpose. 
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(3.8) is con veiled to state equation form as given below. 
Let 


q, = q 


q 


2 


= q 


Then, the state space representation of (3.8) is. 


q; = - CM(q^)]''' { C(q^,q2) + F(q3) - u } 

(3.11) 


Or in a more compact form 


x' = f(x,u) 


where, 

T 

X = [q^ q^] is the state vector, 

x' = tq^ q^l is the derivative of the state vector and 

ftx,u] = [q^ -EM(q^}]’* { C(q^,q2) + G{q^) + FEq^) - u 


The vector z in the performance index may not be directly 
observable from the state equations. So the observer equation 


2 = g(x) 


(3.12) 


is used which constructs the vector z from the state vector x. The 
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form of the function g(-) is different for the different cases 
considered in chapter four and hence is discussed in detail in 
that chapter. 

Having defined the performance criterion, the tracking 
problem can now be mathematically stated as 


t 


Minimize J = - z^Q z + - 

2 f 2 

subject to, 

x' - f(x,u) = 0 
z = g(x). 


(z^Q z + u^R u) dt {3.13(a)) 


(3.13(b)) 

(3.13(c)) 


This is termed as the Optimal Tracking Problem in control 
literature. The above problem is a simple fixed time non-hnecir 
constrained optimization problem because of the non-Unear 
constraint equations (3.13(b)). It can be converted to an 
unconstrained optimization problem using the Lagrange undetermined 
multipliers as 


Minimize ^ \ 


[- (z^Q z + u’^R u) + x’^(f(x,u) - x' )] dt 
2 


(3.14) 


where, 

X is the Lagrange undetermined multiplier-. 
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3.4.2 The Variational Approach 

The fixed time unconstrained optimization problem 
be solved using the Variational Principle. The 
conditions for optimality are obtained in the 
manner (Kirk [1967]). 

First define the Hamiltonian of the system as 

» 

St = (z^Q z + u) + X^f(x,u). 

The necessary conditions of optimality can then be 
conveniently in terras of the Hamiltonian as 


9u 


0 


ax 



for all time t e [o, t^] 


The boundary condition are 


X 


t«0 


X 


o 


(3.14) can 
necessary 
following 

expressed 

(3.15(a)) 

(3.15(b)) 

(3.15(c)) 

(3.16(a)) 


and 
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-'-X 

au 


6x^ = 0. 

f 


(3.16(b)) 


where, 


h, = hi - 2 Q 2 , 

^ Lx f'^r f ' 

1 » t 

f 


2^ = 2 


Qf * Q 


1 « i 




6x^ is the variation on x at the terminal time , 
and 


X = X 

f 


t» t 


. ax 
at ■ 


The set of non-linear differential and algebraic equations 
(3.15) with the split boundary conditions (3.16) form the 
Non-Linear Two Point Boundary Value Problem (NLTPBVP). In general, 
the ancdytical solution for such a problem is not possible and 
even the numerical solution is very involved. An iterative 
numerical scheme used in this work is presented in the next 
section. 

3.5 NUMERICAL ALGORITHM FOR OPTIMAL SOLUTION 

Nedeljkovic [1981] proposed a family of first-order 
iterative algorithms for solving continuous-time unconstrained 
non-hnear optimal control problems. He has also presented the 
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proof of improvement at each iterative step. The method is 
essentially as used for the class of Linear Quadratic Problem. The 
algorithm used in the present work is discussed below. 

Since the algorithm is based on the Linear Quadratic 
Problem, it requires the linearized form of the state and observer 
equations (3.11) and (3.12). The iterative nature of the scheme 
allows the linearization based on the state vector and the input 
vector of the previous trajectory. For the trajectory at the i*'*' 
iteration, the state equations are, 

X = f(x ,u ) 

(i) / (i)x 

2 = g(x ) 

Then the trajectory at the (J+1)*'*' iteration is written by 
expanding it in Taylor series upto the first order terms to get 


, ( i + l) <, ( i ) ( i )v . df 

X = f(x ,u ) + ^ 


, (i+t) (i). ^ df 

(X -X )+3^ 


, (i+l) (i), 

(U - U ) 


or, X 


, (i + l) _ ^ 

" dx 


{ i + 1 ) . df 


. f (i) (i). 

+ f{x ,u } - 


u 

9x 


(i + l) 


(i) 


§f 

dn 


ay 1 

, J 


, (i+l> » . n , 

or, X =Ax +Bu +e 


(3.17) 


where 
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A = 


3x 


(3.18(a)) 


9u 


e'” .f(x'‘',u“') 


a£ 

dyL 


(i ) 


du 


u 


( i ) 


(3.18(b)) 

(3.18(c)) 


Similarly for the observer equation. 


„ ( 1 1 ) / f i ) » . 

2 = g(x ) + 




or, 2 


( i -n ) _ ^ 

ax 


x‘“'’ t 


q(x 


_ §2 
' ax 




(H-D _ (1 + 1) , (1) 

or, 2 = C X + w 


where. 


C = ^ 

^ ax 


eind 


w = g(x‘'’) - ^ 




(3.19) 


(3.20(a)) 

(3.20(b)) 


For notational convenience, (3.17) and (3.19) are rewritten after 
dropping out the superscripts as 
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X = Ax+Bu + e (3.21(a)) 

2 = C X + w (3.21(b)) 

where, 

A is a square matrix, 

B is a rectangular matrix, 

C is a rectangular matrix, 

u is the joint torque vector or input vector and 
e, w are the Unearization error vectors. 

The detailed expressions of the matrices A and B have been 
presented in Appendix B. 

The algorithm can now be stated as follows. 


Step 1 

For iteration index i = 0, assume nominal control and 

numerically integrate the non-linear equations of motion 
(3.11) to get x^°\ 

Evaluate from (3.10). 


Step 2 

Solve the following equations backwards from t^ to 0 

P' + A^P + PA 4- C^QC - PBR'^b’^P = 0 (3.22(a)) 

s' - PBR'^B^s + A^s + C^Q(w - y .) + Pe = 0. 

d 


(3.22(b)) 
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with the boundary conditions, 


P = C’^Q CX, 

•i.t ^ 

f 

s I . c"q,(w, - y ). 

l«l f 


and 


Step 3 

Define 

and 


, (i ) , ( i ) . ( i ) , . 

b = u + R B (Px + s) 


Au 


b"’ E b“> dt 


Set e = 1.0 . 

If Au = 0 then stop. 


Step 4 

Set 


(i+l) (i) 

U = U 


- eb 


(i) 


E'Vpta"'” 


(i)v 
X ) 


and integrate the non-linear equations of motion (3.10)- 
(i + t) 


Evaluate J 
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Step 5 


If J 


(i + l) 


- J 


(i) 


eAu 


(i) 


< 0 


then £ 
Set 


(i-fl) 

( i ) 


= X , 

(i + l) 

( i) 


= U 

(i + l) 

i ) 


= J 


and 


Goto Step 2. } 
else { 



Goto Step 4 } 


The equation (3.22(a)) in the above algorithm is known as the 
Matrix Riccati Differential Equation. It is a non-linear first 
order matrix differential equation. 

This algorithm has been used for solving the present 
problem. Different perfomance criteria have been constructed under 
the framework of Linear Quadratic Problem. The simulation results 
and their anedyses are presented in the next chapter. 
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CHAPTER 4 


NUMERICAL SIMULATION, RESULTS AND DISCUSSIONS 

4.1 INTRODUCTION 

In this chapter, various numerical examples of the 
trajectory tracking problem for a three-link planar manipulator 
have been presented. The algorithm outlined in Sec. 3.5 has been 
used to solve the optimal tracking problem. For the purpose of 
comparison, the pseudo-inverse solution was obtained using 
(2.3(a)) in Sec. 2.2. The results of the numerical experiments 
performed have been presented. The computer implementation of the 
algorithm has been in the C language and the program was executed 
on the HP9000/850S machines. The Starbase graphics library was 
used to generate the plots of the results. 


4.2 ROBOT PARAMETERS AND TRAJECTORY SPECIFICATIONS 

The various parameters values assumed for the manipulator 
shown in Fig. 3.1(a) are tabulated below. 


n 

ra. (kg.) 

1^ (m) 

P. (m) 

X 

I. (kgm^) 

1 

5.0 

1.0 

0.5 

0.41667 

2 

3.0 

1.0 

0.5 

0.250 

3 

2.0 

1.0 

0.5 

0.16667 
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Type A : 

Radius of the circle = 0.50 m 
Position of the center : (1.50, 1.50) ra 

Type B : 

Radius of the circle = 1.0 m 
Position of the center : (1.0, 1.50) m 

The time of travel for both the trajectories were fixed at 10.0 
seconds. The initial configuration of the manipulator for both the 
trajectories were taken as 

0^ = 1.43047 rad 6^ = -0.89521 rad 0^ = -0.535492 rad 

Case 2 *. Linear trajectory 

Starting point *. (1.6, 1.0) m 
End point : (-1.6, 1.0) ra 

The time of travel was fixed at 8.0 seconds. The initial 
configuration for this case was 

0^ = 2.214296 rad 0^ = -1.287 rad 03 = -0.927294 rad 

For the purpose of numerical simulation, the trajectories 
were specified as a set of closely spaced points in the workspace. 
For numerical computation purpose, in each case, 2000 equispaced 
points were considered over the trajectory. 



Chapter 4 Numerical Simulation, Results and Discussions 


51 


4.3 RESULTS AND DISCUSSIONS 

In this section the various numerical experiments performed 
have been discussed. Different types of performance indices were 
constructed to investigate the efficacy and versatility of the 
algorithm in solving the optimal control problem. In each case the 
nominal input, required at the start of the algorithm, was assumed 
to be zero, i.e. 


u 


( 0 ) 


0 . 0 . 


The numerical integration of the differential equations involved 
in the algorithm, as outlined in Sec. 3.5, were performed using 
fourth order Runge-Kutta integration scheme with fixed step 
size. Results of the experiments have been presented in the form 
of plots and the various observations discussed. 


4.3.1 Circular Trajectory Tracking udth no Bounds on Joint 
Torques and Joint Velocities 

In this sub-section, four cases are presented as follows. 


CASE 1 

In this case, simple tracking of the trajectory is 
considered. No limits are imposed on the range of joint motion. 
The performance index was of the form 



A. R361 
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= i + i 


( z^Q z + u^R u ) dt 


(4.1) 


where. 


z = [ e q']^, 

e is the trajectory error vector, 


z = z 

f 


t-t 


u is the joint torque vector, 
tj, = 10.0 sec, is the time of travel, 

Qj., Q, R are the positive definite weighting matrices, 
Q = dlag [Q^ Q^] and 

Qf = Q 


1* t 


The sub-matrix, is of dimension 2x2 while, is of dimension 
3x3. 


The observer equation (3.6) mentioned in Sec. 3.4 in this 
case was defined as 

Z^ = IjCOS(e^) + l2COS(0^3) + l3COS(e,23) 

Z3 = l^sin(0^) + l2Sin{e^^) + l3Sin(0^33) 



2 0' (4.2) 

S 3 1 

where, 

1^ is tne length and 
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^i,^i joint variable and joint velocity 

respectively. 

The weighting matrices chosen were 

Qj. = diag E6.0E3 6.0E3 8.0 8.0 8.0] 

Q = diag [2.0E4 2.0E4 0.0 0.0 0.0] 

R = diag [1.0 1.0 1.0] 

The sub-matix of Q was chosen with all elements as zero 
implying thereby that there is no penalty on the joint velocities 
adong the trajectory. Whereas, in order to reduce the joint motion 
at the end of the trajectory, peneilty has been imposed on the 
joint velocities at the final time through Q^. 

The simulation runs for type A circular trajectory was 
taken. The results are presented in form of plots in Figs. 
4.1{a)-(f}. The configurations at six instants eure shown in Fig. 
4.1(a) along with the generated end-effector trajectory. With 2000 
points taken on the end-effector trajectory, each configuration in 
the figure is taken at cui interval of 400 points and numbered 
sequentially. A comparison of the generated and the desired 
trajectory is presented in Fig. 4.1(b). The variation of joint 
angles, joint velocities, end-effector velocity and joint torques 
with time are plotted in Figs. 4.1(c)-(f) respectively. 
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FIG. 4.1<a> MANIPULATOR CONFIGURATIONS IN OPTIMAL TRACKING 

WITHOUT BOUNDS ON JOINT VELOCITIES AND 


JOINT TORQUES 
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0.0 10 2.0 3.0 4.0 5.0 6.0 7.0 8.0 9.0 10.0 


Time (sec) 

FIG. 4.1<c> VARIATION OF JOINT ANGLES VITH TIME IN OPTIMAL 
TRACKING WITHOUT BOUNDS ON JOINT VELOCITIES 


AND JOINT TORQUES 



Time (sec) 


FIG. 4.1 Cd> VARIATION OF JOINT VELOCITIES VITTI TIME 

IN OPTIMAL TRACKING WITHOUT BOUNDS ON JOINT 
VELOCITIES AND JOINT TORQUES 




FIG, 
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FIG. 4.1Ce> VARIATION OF END-EFFECTOR VELOCITY WITH TIME 


IN OPTIMAL TRACKING WITHOUT BOUNDS ON JOINT 
VELOCITIES AND JOINT TORQUES 



TRACKING WITHOUT BOUNDS ON JOINT VELOCITIES 

AND JOINT TORQUES 
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It is observed from the joint velocity plot (Fig. 4.1(d)) 
that there is an initial surge in the joint velocity implying a 
high initial acceleration. This is very prominent in the third 
joint of the manipulator. The joint torque plot (Figs. 4.1(f)) 
also exhibits a high initial joint torque for the third joint. The 
iniUal configuration is such that the third joint is most 
effective in producing the initial end-effector motion which is 
vertically upwards. As a result of this, the third joint flips 
position from negative to positive joint angle. The end-effector 
also attains a high initial velocity as reflected by the initial 
surge in the end-effector velocity plot in Fig. 4.1(e), which is 
otherwise uniform over the rest of the motion. 

It is also observed from the joint velocity plot (Fig. 
4.1(d)) and end-effector velocity plot (Fig. 4.1(e)) that at the 
terminal time, a small but finite velocity persists. This is due 
to the fact that the problem has been solved considering free 
terminal states while putting weightage on the terminal states. 
Hence by increasing the weightage on the joint velocities at the 
terminal time, terminal velocities cam be reduced. It has been 
observed though, that the weights cannot be increased arbitrarily 
as it tends to make the backward numerical integration of the 
Eiccati equation (3.22(a)) unstable. In order to arrest the motion 
of the manipulator completely at the final time, brakes may be 
applied which is done often in practice. 

It is observed from Fig. 4.1(a) that the final configuration 
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is different from the initial configuration. This phenomenon is 
referred to as drift in configuration or non-conservative motion 
of the manipulator. This is particularly observed in case of 
control schemes based on local optimization in redundant 
manipulator (Klein and Huang [1983]). While performing cyclic 
tasks, configuration drift may result in collision with obstacles 
in the robot workspace and hence is an undesirable feature. 

In order to compare the problem of drift, the inverse 
kinematic solution was obtained using the pseudo-inverse of the 
Jacobian matrix of the manipulator as was outlined in Sec 3.3. In 
this case, the circular trajectory of type B was considered. The 
mainipulator configuration at six instants over the trajectory, 
obtained through the pseudo-inverse solution, is showii in Fig- 
4.2(a) and the joint angle and joint velocity variation are 

plotted against time in Figs. 4.2(b) aind (c) respectively. It has 
been conclusively proved (Klein and Huang [1983]) that in planar 
redundant manipulators, involved in tracking a cyclic trajectory, 
drift is unavoidable using the pseudo-inverse solution. Figures 
4.2(d) and (e) show the configurations as obtained by the 

pseudo-inverse solution and the optimal control solution 
respectively, at the end of each cycle of the circular trajectory. 

It is clear from Fig. 4.2(d) that the drift in the 

end-effector configuration through pseudo-inverse solution is 
unstable in nature that is, the configuration keeps drifUng from 
cycle to cycle without settling down on a definite configuration. 



FIG. 4.2Ca> MANIPULATOR CONF IGURAT I ONS IN TRACKINC3 U5:iNG 


PSEUDO- INVERSE SOLUTION 
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SE SOLUTION 
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On the other hand, for the present algorithm it approaches a 
limiting configuration after only two cycles as seen in Fig 
4.2(e). So an initial configuration can be determined which will 
result in a conservative motion of the manipulator. 

The stability of drift in configuration was also observed in 
case of the circular trajectory of type A. The results, when the 
limiting configuration was reached, are presented in Figs. 
4.3(a)-(f). The configurations of the manipulator at six instants 
over the trajectory are shown in Fig. 4. 3 (a). A compctclson of the 
generated and desired trajectories is presented in Fig. 4.3(b). 
The joint angle plot against time is presented in Fig. 4.3(c). 
Variation of joint velocities, end-effector velocity and joint 
torques are presented in Figs. 4.3(d)-(f) respectively. Starting 
from this configuration it is observed from Fig. 4.3(a) that the 
flipping of the third joint is absent. Moreover the initiad surge 
in joint velocities (Fig. 4.3(d)) and the end-effector velocity 
(Fig. 4.3(e)) is comparitively low as observed respectively in 
Figs. 4.1(d) and (e). It is however to be noted that this initial 
configuration which results in conservative manipulator motion may 
not be unique as is evident from Fig. 4.3(g) which is a plot of 
the stable configuration obtained using the present algorithm, for 
the circular trajectory of type A, starting from an initial 
configuration different from the that in Fig. 4.1(a). 
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FIG. 4.3<ci> MANIPULATOR CONF I GURA T I ONS IN OPTIMAL TRACKING 


STARTING FROM A LIMITING CONF 1 GlIR AT I ON 




ED END- EFFECTOR 


STARTING FROM A 






EE Velocity (m/sec) 
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FIG. 4.3<«> MANIPULATOR CONFIGURATIONS IN OPTIMA!, TRACKING 

STARTING FROM A DIFFERENT LIMITING CONFIGURATION 
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CASE 2 

In this case, the problem of trajectory tracking with a 
specified terminal configuration of the manipulator, has been 
considered. The terminal configuration was assumed to be same as 
the initial configuration in order to generate a conservative 
manipulator motion over the circular trajectory of type A. 

The performance index was restated in the form 


J = 


2 f 2 


z^Q z + u^R u ) dt 


where, 

z = [ e (q - q^) q'3^, 

a 

e is the trajectory error vector, 

T 

q = [©i ©2 joint variable vector, 

q^ is the desired joint variable vector, 



u is the joint torque vector, 
t = 10.0 sec, is the time of travel, 

f 

Q^, Q, R are the positive definite weighting matrices 


and 



( 4 . 3 ) 


The condition imposed on the joint variable vector at the terminal 


time was 
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t=t ' t»o 

f 


The observer equation (3.6) mentioned in Sec. 3.4 in this 
case weis defined as 


2^ = ljCOS(0^) + l2cos{0^2> + 
2^ = l^sin(0^) + l2Sin(0^2) + 


where. 


z 

sz 

0 

- 0 

3 


1 

Id 

Z 

SE 

0 

- 6 

4 


2 

2d 


SB 

0 

- 0 

5 


3 

3d 


ss 

0' 


6 


1 


2 

= 

8 ' 


7 


2 


2 


0' 


8 


3 


If 





• 4.V. •‘•h 

is the i 


0 6' are the i^^ joint variable and joint velocity 

i , 1 . 

respectively. 


The weight matrices in this case were chosen as 


= diag [20.0 20.0 2.0E2 2.0E2 2.0E2 8.0 8.0 8.0] 
Q = diag [2.0E4 2.0E4 0.0 0.0 0.0 0.0 0.0 0.0] 
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R = diag [1.0 1.0 1.0] 

The third, fourth and the fifth diagonal elements in impose the 
penalty on the error of the actual and desired terminal 
configurations. 

The results of the simulation are presented in Figs. 
4.4(a)-(e). The configurations at six different instants over the 
trajectory are presented in Fig. 4.4(a). The plots of joint 
angles, joint velocities, end-ffector velocities and the joint 
torques against time are shown in Figs. 4.4(b)-(e) respectively. 
It can be seen from Fig. 4.4(a) that the final configuration 
matches exactly with the initial configuration. The joint 
velocities at the terminal time are also quite low. 
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G. 4,4Ca> 



MANIPULATOR CONFIGURATIONS IN OPTIMAL TRACKING WITH 


SPECIFIED TERMINAL CONFIGURATION 
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CASE 3 

In this case, the trajectory tracking problem with 
the minimization of kinetic energy of the manipulator hcis been 
solved. The performance index in this case is the same as given by 
(4.1) and the observer equation (3.6), as mentioned in Sec. 3.4, 
is defined to be same as given by <4. 2) in Case 1 above 

The weighting matrices in the performance index in this case 
are chosen as follows. 

R = diag [1.0 1.0 1.0] 

Qj. = diag [6.0E3 6.0E3 8.0 8.0 8.0] 

= diag [2.0E4 2.0E4] 

Q 2 = [M(q)] 
where, 

M(q) is the inertia matrix of the manipulator. 

The term of the form q'^tM{q)]q', obtained on simplifying the 
performance index (4.1), minimizes the kinetic energy over the 
trajectory. 

The results of the simulation for the circular trajectory of 
type A are presented in Figs. 4.5(a)-(e). The Fig. 4.5(a) shows 
the manipulator configuration at six different time instcints over 
the trajectory cind the joint angle variation with time is shown in 
Fig. 4.5(b). The joint velocity, end-effector velocity and joint 
torque profiles over the trajectory is plotted against time and 
presented in Figs. 4.5(c)-(e) respectively. 
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4.5<«i> 



MANIPULATOR CONFIGURATIONS IN OPTIMAL TRACKING WITH 


MINIMIZATION OF KINETIC ENERGY 


ao 9.0 10. 


WITH TIME 
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. 4 , 5 <©> 


Numerical Simulation, Results and Discussions 



VARIATION OF END-EFFECTOR VELOCITY WITH TIME IN 
IPTIMAL TRACKING WITH MINIMIZATION OF KINETIC ENERGY 



VARIATION OF JOINT TORQUES WITH TIME IN OPTIMAL 


OF kini:ti<: i^nergy 
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Comparing with the joint velocity plot in Figs- 4.1(d) for 
simple path tracking, it is observed that in this case level of 
the joint velocities (Fig. 4.5(c)) over the trajectory are low but 
fluctuating. At the end of the trajectory the joint torques, as 
shown in Fig. 4.5(e), are compciratively higher and are such, that 
the terminal motion of the manipulator is almost stopped. The 
results are reasonable considering that the kinetic energy is 
minimized. 

CASE 4 

In this case, the tracking of the specified trajectory with 
constraint on the range of motion of the joints has been 
considered. In most practical situations, the joints of a 
manipulator cannot rotate beyond a certain range because of design 
considerations. This constraint has been incorporated into the 
tracking problem not as a mathematical constraint in the 
optimization process but as an external constraint on the dynamics 
of the system. The external physical constraint is imposed at the 
joint limit in such a way that when the joint reaches its limit, 
it comes in contact with a torsional spring and a dashpot as shown 
in Fig. 4.6. The equations of dynamics of the manipulator, as 
presented in Sec. 3.2, are modified by introducing a function 
v(q, q'f q ) which has been defined below. The modified 
equations of motion are expressed in the standard notations as 
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[M{q}] q" + c(q, q') + F{q'} + v(q, q', q,. ) = u (4.4) 

I im 

where, 

' "ii.’ “(*!<<»- + d q' ] flq, 

/(q. q„„>- 1.0 111 > ll.lJ 

0-0 otherwise. , 

is the joint angle limit, upper or lower as the caise 
might be, 

k, d are termed as the constraint stiffness and damping 
respectively. 

The performance index in this caise was the same as given by 
(4.1). The observer equations (3.6), ais mentioned in Sec. 3.4, are 
also defined to the same as given by (4.2). The weight matrices 
chosen case were 

Qj. = diag [6.0E3 6.0E3 8.0 8.0 8.0] 

Q = diag [2.0E4 2.0E4 6.0 6.0 6.0] 

R = diag tl.O 1.0 1.0] 

The last three diagonal elements of the Q matrix imposed certain 
weights on the norm of the joint velocities. This was found to be 
necessary in order to stabilize the numerical integration 
procedure of the differential equations involved in the algorithm 
as outlined in Sec. 3.5 when the joint reached its limit and came 
in contact with the torsional stiffness and dashpot. 



4 Numerical Simulation, Results and Discussions 
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The joint limit was imposed on first joint of the 
manipulator tracking the circular trajectory of type A. From Fig. 
4.1(c), it is observed that the first joint makes a maximum angle 
of about 1.84 radians (105.42 ). So the joint upper limit was put 
at 1.65 radians (94.53 ). The constraint stiffness and damping 
were taken as 

k = 100 N/rad 
d = 200 Ns/rad 

The plots of the results eire presented in Fig. 4.7(a)-(e). The 
configurations at six different instants over the trajectory is 
shown in Fig 4.7(a). The joint angle variation with time is 
plotted in Fig. 4.7(b). The joint velocity, end-effector velocity 
and the joint torque variation with time is presented in Figs. 
4.7(c)-{e). 

The joint angle plot (Fig. 4.7(b)) and the joint velocity 
plot (Fig. 4.7(c))indicate that the first joint reached its upper 
limit of 1.65 radians (94.53°) and remained stationary over a 
period. There is a discontinviity in the slope of joint velocity 
curves of all the joints implying a jerk at the joints. This is 
due to the fact that the joint constraint imposed on the dynamics 
of the manipulator becomes active in a sudden manner. At that 
point of time, the motion of the other two joints are readjusted 
to maintain the tracking. There are clear signs of control torque 
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FIG. 4.7<ei> MANIPULATOR GONF 1 GURAT IONS IN OPTIMAL TRACKING WITH 

JOINT CONSTRAINT 
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chatter in the joint torque plot {Fig. 4.7(e)) at the point of 
contact of the joint with the constraint. This is possibly to 
prevent oscillations because of the stiffness of the constraint. 

4.3.2 Linear Trajectory Tracking Mth no Bounds on Joint Torques 
and Joint Velocities 

In this sub-section, two cases are presented as discussed 

below. 


CASE 1 

In this case, a simple trajectory tracking example, with no 
joint constraint, over a linear trajectory has been considered. 
The specification of the trajectory is presented in Sec. 4.2. The 
performance index was defined in the form 




IT 1 

J = - 2 0 2 + - 

2 f^f f 2 


( z^Q z + u^R u ) dt 


(4.5) 


where, 

z = [ e q']^, 

e is the trajectory error vector. 



u is the joint torque vector, 
t^ =■ 8.0 sec, is the time of travel, 

Q^, Q, R are the positive definite weighting matrices. 
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Q = diag [Q^ Q^] and 


Qf = Q 


t * i 

f 


The sub-matrix, is of dimension 
dimension 3 x 3 . 


2 x2 while, is of 
“2 


The observer equation (3.6), as mentioned in Sec. 3.4, in 
this case was defined to be same as given by ( 4 . 2 ). 

The weight matrices considered in this ceise were 


= diag [6.0E3 6.0E3 8.0 8.0 8.0] 

Q = diag [2.0E4 2.0E4 0.0 0.0 0.0] 

R = diag [ 1.0 1.0 1 . 0 ]. 


The results of the simulation etre presented in Figs. 
4.8(a)-(e). The manipulator configurations at six instants over 
the trajectory are plotted in Fig. 4.8(a). The joint angle 
variation with time is presented in FiQ.4.8(b). The joint 
velocity, end-effector velocity and joint torque variation with 
time are plotted in Figs. 4.8(c)-(e). 

In case of the linear trajectory, the high initiad joint 
velocity (Fig. 4.8(c)) and joint torque (Fig. 4.8(e)) of the third 
joint is observed. The initial trend is similar to that seen in 
case of circular trajectory (Figs. 4.1(c) and (e) respectively). 
It is however observed that the joint torques (Fig. 4.8(e)) are 
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FIG 4.8<a> MANIPULATOR CONFIGURATIONS IN OPTIMAL TRACKING 
WITHOUT BOUNDS ON JOINT VELOCITIES AND TORQUES 


Joint Velocity (rad/sec) v Joint Angle (rad) 


Chapter 4 Nu 

6.00 r 



FIG. 4.0<c> VAR] 




Chapter 4 N\ 


FIG. 


o 

(D 

tn 


Q) 

> 


lii 


0.50 


0.40 


0.30 


+j 


020 


0.10 


0.00 

0. 


4.0<d> VAF 


200 

100 


0.00 


-100 


& -^00 


.5 




“3.00 


“4.00' 


- 5.00 


“ 6.00 


-7.00 


FIG. 4.0<o> V 





Chapter 4 Numerical Simulatiotir Results and Discussions 


92 


low compared to those for the circular trajectory (Fig. 4.1(e)). 
This is possibly due to the linearity of the trajectory which is 
free from any cheinge in direction of the end-effector motion 
unlike that in the case of the circulcir trajectory. 

The simulation results for the linear trajectory tracking 
using the pseudo-inverse solution is presented in Figs. 
4.9(a)-(c). The configurations at di ff erent instants over the 
trajectory are presented in Fig. 4.9(a). The joint angle and joint 
velocity variation with time are presented in Figs. 4.9(b) and 
(c) respectively. 

The manipulator configurations obtained using the 
pseudo-inverse solution (Fig. 4.9(a)) is markedly differrent than 
those obtained using the optimal control algorithm (Fig. 4.8(a)). 
For the pseudo-inverse solution, it was found that for repetative 
motion over the linear trajectory (that is for to and fro 
motion), the configurations for the forward and reverse nations 
are exactly repeated. Using the present algorithm, it was observed 
that after two cycles of repeatation of the tracking, a limiting 
configuration was reached which had no further drift. The results 
for this observation have not been presented though. 



FIG 4.9<a> MANIPULATOR CONFIGURATIONS IN TRACKING USING 

PSEUDO- INVERSE SOLUTION 
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CASE 2 

The linear trajectory tracking example with limits on the 
range of motion of the joints has been considered in this case. It 
was mentioned earlier that because of the design considerations, 
the joints of the manipulator can execute only limited motion 
range. This condition was imposed using an external constraint on 
the dynamics of the manipulator as was mentioned in Case 3 of Sec. 
4.3.1. The equations of motion of the manipulator are therefore 
the same as given by (4.4). 

The observer equation (3.6),as mentioned in Sec. 3.4, in 
this case was defined to be same as in (4.2). The weight matrices 
were chosen as 

= diag [6.0E3 6.0E3 8.0 8.0 8.0] 

Q = diag t2.0E4 2.0E4 6.0 6.0 6.0] 

R = diag [1.0 1.0 1.0] 

The last three diagonal elements of the Q matrix imposed certain 
weights on the norm of the joint velocities. This was found to be 
necessary in order to stabilize the numerical integration 
procedure of the differential equations involved in the algorithm 
as outlined in Sec. 3.5 when the joint reached its limit and came 
in contact with the torsional stiffness and dashpot. 

In the simple tracking of a linear trajectory, it is 
observed from Fig. 4.8(a) that the second link folds onto the 
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first link during the travel. In order to prevent this, a lower 
joint angle Umit for joint two was set at -1.65 radians (-94.53*) 

on the second joint. The constraint stiffness and damping in this 
case were taken as 


k = 600 N/rad 
d = 800 Ns/rad 

The results are presented in Figs. 4.10(a)-{e). The 
manipulator configurations at six instants over the trajectoy are 
presented in Fig. 4.10(a). The joint angle variation over the 
trajectory is shown in Fig. 4.10(b). The joint velocity, plotted 
against time is shown in Fig. 4.10(c) whUe Figs. 4.10(d) and (e) 
are the plots of the end-effector velocity and joint torque 
variation with time. 

The joint angle plot in Fig. 4.10(b) shows that the second 
joint just reaches its lower limit. At that instant the joint 
velocity reverses direction as observed in Fig. 4.10(c). Since, in 
this case, the second link could not fold onto the first, the 
manipulator configurations over the trajectory (Fig. 4.10(a)) are 
quite different that those observed in Case 1 (Fig. 4.8(a)). The 
manipxjtlator motion in this case is smooth as compared to those 
observed in the case of the circular trajectory with joint 
constraint as in Fig 4.7(b)-(e). 
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FIG. 4.10<a> MANIPULATOR CONFIGURATIONS IN OPTIMAL TRACKING 


WITH JOINT CONSTRAINT 
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4.3.3 Circular Trajectory Tracking with Bounds on Joint Torques 
and Joint Velocities 

In the examples discussed above, no bound on the joint 
velocities or torques were prescribed. In aill practical cases 
however, there are definite bounds on the joint velocities and 
torques from the design considerations and actuator ratings. In 
such cases, the joint torques obtained from the optimal control 
cdgorithm may be infeasible to achieve. This aspect has been taken 
care of by using a penalty approach as described below. 

In this case the performance index was of the form 


J = 


^ T- .1 

— z Q 2 + - 
z r 2 


z^Q z + u^R u ) dt 


( 4 . 6 ) 


where. 


z = [ e q']^, 

e is the trajectory error vector. 



u is the joint torque vector, 
t = 10.0 sec, is the time of travel, 

f 

Q^, Q, R are the positive definite weighting matrices, 
Q = diag [Q^ Q^] and 



The sub- matrix, is of dimension 2x2 while, is of 


dimension 3x3. 
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The observer equation {3.6),as mentioned in Sec. 3.4, in 
this case was defined to be same as in (4.2). The weight matrices, 
in this case, were defined as 

Rii = + ar i = 1, 3 

^211 ~ ^211 i = 1, 3 

where, 

Rjj is the element of the weight matrix on the joint 
torques, 

Qg j i is the element of the weight matrix on the joint 
velocities, 

R^ j , Q 2 i i the nominal values of the weights and 

AR» AQ^ are the penalty weights. 

The function g(-,-) is defined as 

g(a, b) = 0.0 a < b 

= 1.0 a i b. 

In the present case, the limiting values of the joint 
velocity and torque are set on the basis of the results obtained 
in the simple tracking in Case 1 of Sec. 4.3.1. The numerical 
values are taken ais 

R°. = 1.0 


AR = 100.0 



Chapter 4 Numerical Simulation, Results and Discussions 


102 


^ 1 . - 
oL, = oo 
- 100.0 

q, . = 0.80 rad/sec. 

The results of the simulation are presented in Figs. 4.11{a)-(e). 
In Fig. 4.11(a), the manipulator configurations are piotted at 
regular time intervals at six instants over the trajectory. The 
joint angle is plotted against time in Fig. 4.11(b). The variation 
of joint velocities, end-effector velocity and joint torques with 
time are presented in Figs. 4.11(c)-(e) respectively. 

From the joint velocity plot (Fig. 4.11(c)) and the joint 
torque plot (Fig. 4.11(e)), it is observed that the bounds are 
indeed maintained by the algorithm. 
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FIG. 4.11<b> VARIATION OF JOINT ANGLES WITH TIME IN OPTIMAL 


TRACKING WITH BOUNDS ON JOINT VELOCITIES 
AND JOINT TORQUES 
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CHAPTER 5 


CONCLUSIONS AND SCOPE OF FUTURE WORK 


5.1 CONCLUSIONS 

In this work the trajectory tracking problem of redundant 
manipulators was investigated. The problem was posed as an Optimal 
Tracking Problem with fixed initial states, free terminal states 
and fixed terminal time. The redundancy was resolved by minimizing 
an integral performance index which was constructed using the 
weighted norms of the tracking error and the Joint torques. The 
equations of motion of the manipulator were taken as the 
constraint equations. Using the Variational Principle, the problem 
was reduced to a NLTPBVP which was solved using an iterative 
numerical scheme. 

The study was made on a three-link planar manipulator. Two 
types of trajectories, a circular and a linear, were considered. 
Various types of examples were considered and solved using the 
algorithm. Some of the results obtained using this approach were 
compared with the corresponding pseudo-inverse solutions. 

In particular, the examples were classified in three broad 
categories. The conclusions drawn are as follows. 

In the first category, the tracking of a circular trajectory 
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with no bounds on the joint torques or joint velocities was 
considered- It was observed, from the various cases considered, 
that the algorithm is suitable for tracking a trajectory as well 
as meeting additional requirements such as attaining a desired 
terminal configuration, kinetic energy minimization and satisfying 
the constraint on the range of joint motion. It was observed that 
the problem of drift in the configuration for cyclic taisks is 
present but is stable in nature unlike In the case of the 
pseudo-inverse solution where it is unstable. A stable starting 
configuration for such cyclic tasks could be obtained after a few 
cycles of the task. Another approach to prevent drift was ailso 
presented in which the terminal configuration was specified to be 
same as the starting configuration. The results obtained were 
quite satisfactory. 

In the second category, the tracking of a linear trajectory 
Wcis considered. In this case two sub-cases, a simple tracking 
without any constraint on the motion of the joints and tracking 
with joint motion range limits, were presented. The results 
obtained serve to confirm the conlusions drawn for the tracking of 
the circular trajectory. 

In the third category, the circular trajectory tracking 
problem was considered with bounds on the joint torques and joint 
velocities. The resiilts obtained indicate that the bounds were 
satisfied by the optimal solution obtained. 
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APPENDIX A 


A.1 THE MINIMUM NORM SOLUTION 

Consider the motion rate equations of a redundant 
manipulator, as discussed in Sec. 2.2, of the form 

y' = J q' (A.1) 

where, 

y' is the end-effector velocity vector, 

J is the Jacobian matrix of the manipulator and 
q' is the joint velocity vector. 

The Jacobian matrix of a redundeint manipulator is a 
rectangular matrix and hence is not ordinarily invertible. So, in 
order to determine the joint velocities when the end-effector 
velocity is specified, the following optimi 2 ation problem is 
posed. 


Minimi 2 e 

J = q'”^ W q' 

subject to 

y' = J q' 


where, 
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W is a positive definite symmetric weighting matrix. 

This constrained optimization problem is converted to an 
unconstrained optimization problem using Lagrange undetermined 
multiplier as 


J = q''^ W q' + ( J q' - y') 

where, 

X is the Lagrange undetermined multiplier. 


(A.2) 


The necessary condition for (A.2) to be minimum are given by 


dj 

dq' 


= 0 


and 


aj 

ax 


0 


The above conditions imply that 
2 W q' + j'^ X = 0 

or, q' = I W'^J X {A.3a) 

and 

J q' - y' = 0 (A.3b) 


Substituting the expression for q' from (A.3a) in (A.3b) yields 
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y' - I J w-*j X 

or, X = 2 [J y' 

Substituting this expression for X in (A.3a), the joint velocity 
vector is expressed in terms of the end-effector veclocity vector 
as 

q' = W"V [J W‘V]'^ y' 
or, q' = J* y' 
where, 

J* = W'V [J is known as pseudo-inverse of the 


Jacobian matrix. 
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B.l STATE-SPACE FORMULATION AND LINEARIZATION OF THE EQUATIONS OF 
MOTION OF A THREE-LINK PLANAR MANIPULATOR 
Ths general representation of the equations of motion of the 
three-link planar manipulator, as derived in Sec. 3.2 is given by 

[M(q)]q'’ + C(q,q') + F(q' ) - u (B.l) 

where, 


q 

= t 

d 

1 



is 

the 

joint 

variable vector. 

q' 

= [ 


e' 

2 

e;]^ 

Is 

the 

joint 

velocity vector. 

q" 

= [ 

1 

e" 

2 


is 

the 

joint 

acceleration vector. 

u 

= [ 

X 

1 

T 

2 


is 

the 

joint 

torque vector. 


M(q) is the inertia matrix of size 3x3, 

C(q,q') is the Coriolis and centrepetal force vector of size 
3x1, 

F(q' ) if the friction force vector of size 3 x i. 

The matrix elements are detailed below. 

+ ^2 + ^3 + + 2m3l2P3COS{e3) + 

msPg + ni3l^l2COs(e2) + m^l^p^cosie^) + + m^p^ + 

™23^1 + mglgl.cosce^) + m^p^l^cosce^) + ni3P3l^cos«?33) 
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'^ 12 ^' ^2 + Ig + 2 m 


) + rn .l 2 

^3 3 / f m P + lyj |2 




'" 3 *a‘.cos,»^, . ™,P,l,cos,. , , 


WlC^S( 0 ^^) 


^13* ^22 '*■ ’^r.P^ + m n 1 

3 3 3 ^P^I^COSO^) + 1 

21 ^2 ■*■ I3 + m 1 p cos{0 ) + 2m ) 

^3 ^'"3i2^<=os(e3) + m p3+ 

‘"a^^z^^^sCe ) + jjj . ) . 2 ^ ^ 

M . 1 ^ + m i3 + 2 

*^22 ^2 + ^3 + 2m 1 p cosfO 4 ^ 2 ^ 

3 3'>3«,= (93) + , 3 ^ ^ 

^3 ^ ^P^ 

^3 ^ *’™3^iP 3COS(0 ) + Om 1 

23 ^m^l^p^cosCO^) + n,^p2 

” 33 - ^3 + %I^P^COB(e^) t „ p3 

«33= ^3 ^ .3P3^ 


’ - ™3-.P3«; Ptn,a^3, . »3l3P3e;3,,„, , 

^33V^ Pln.a^, _ .3P3,^,;3^3.„,a3, . 

S - ■” 3 ^P 3 ^^pin,e^ 3 , , » 3 , 3 p^e;^^ 3 ,„,a, , 

“aaVa®, Pin, 6^, . m3l3p^a;|^3i„,a^, _ 

z 

^3 “ *^3 ^ 1^0®! ^sin (0 ) + m I ^<2 

3 13 , "''^ 23 ; + m 3 l 3 P 30 ;^"sin( 0 ^) 

The conversion of the equations of mor . 

Ptate apace form ha h 

form haa been presented In chapter three. Th 
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state-space 


representation. 


as 


Criven by (3.11) 


3.4 is 


Qi = q 

= CM(q^)]'i 


- F(q^, + 


Where, 


(B.2) 


q, = [B e e . 

^2 0']^ , ^tor and 

^ ‘= ‘he Joint velocity veo. 


Cor. 


Let 


=" f 1, q^ 1‘ 

“ [ 0, 0 $ a, 

' ‘ 3 «, e; e-,-’ 

“ ^ r V V „ T 

^ 2 3 \ j\ 


Then (B.2) above can be 


'written as 


{B.3) 


where. 


X' == f{x, u) 


f(x,u) - [q^ 


^ ^ - F(q^) + ^ 


}]‘ 


(B.4) 


«,,n) 


Where, 
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r(x, u) = [ 

= [- C(q^, q^) - FCq^) + u]. (B.5) 

The state equations above are linearized based on a nominal 
trajectory. The linearization scheme has been presented in Sec. 
3.5. The linearized equations are represented as 


x' = Ax + Bu + e 


(B.6) 


• th 


where, the A and B matrices are defined over the i trajectory as 



and 


B = 


df 

9u 


Let 


A = [ A 


A_ A. ] 

5 o 


where. 


Aj represents the row of A matrix. 


The row elements of the matrices A and B can be derived from the 
definition above in (B.6) as described below. 


The first three rows of the A matrix are qiven by 
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r a 










[ ^ 

Cf^(x, 

u)] 1 = 

[ 0 

0 

0 

1 

0 

0 ] 


f d 









^2 = 

[ ^ 

[f^tx. 

u)] J = 

[ 0 

0 

0 

0 

1 

0 ] 

^3 = 

f ^ 

[ ax 

1X3 (X, 

U)] = 

[ 0 

0 

0 

0 

0 

1 1 


where, 

f j (x, u) is the element of f (x, u) defined in (B.4). 

The jf*'*' element of the vectors A. A and where j varies from 1 

4 5 6 

through 6 are given as follows. 






* «33 


- M 


32 23 


(M_) - M 


23 32 


+ ®,2 aj '■^3> 


^ >^3 «32 h *».3' * M.3 I;,'«32> ' ".2 l.<”33> 

V i . i ^ ^ 


- M„„ tt: (M. J } + ^^(r3) + r 


‘33 clx^' 12 




+ »23 S/»,2> - ”22 - ”l3 S,‘”33’ 


- 'Bit'll + ® 22’'2 + ® 33 ‘' 3 > 




22 S/»33> 


+ M33 M33 |; (M33)- m 


a Mo, > 
23 8X3 32 

i i 


22 33 


+ ( M_M_ - M32M23 ) + ^ (M^^) - M^3( ^ {M33) 

i ' 
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^33 dx.^^Zl^ ~ ^31 ^23 aXj^^31^^ 

- ( ) + 1 ;; (M,J - M_( M_ ^ (M_) 


+ M 


21 33 

d 


ax^'“i 2 ' 


* 13 ' “21 aXj “ 32 ' 


32 ax.<” 2 i> - “at I (Maa* ’ ”22 S <”3,’' 

All 


+ ^ *^21*^32 “ ^ (M. J 


31 


axj “ 13 ' 


]] 


”^2 [^ (» 2 , { 


M 3 , S/Maa’ + ”23 fe/Ma,' 


- "2, fe/Maa' - M33 l/Ma,’ | + »22 

+ '^a { M„ S/Maa' + ”33 s/”,,’ ‘ "a, li/M,3> 
M^a ^ I + B^a ^ (J^a) + 2^3 / ^21 

i ; i V I 

+ M ,3 S <M2.' - M,. S.'Maa’ " ”23 l,'M,.> } ) 

i 1 X J ' 

- + B_r, + (M33) 


21 1 


22 2 


+ Maa S/Maa' " "aa fc/Maa' ' "33 fc/Maa" 


+ { M_M_ - M_M_ ) + ^ - M, 2 ( ^^^ 33 ^ 


22 33 

d 


32 23 


■*■ *^33 aXj^^Zl^ ~ *^31 ax. ^*^ 23 ^ " *^23 ax.^'^Sl^^ 


i 


- ( M„.M_ - M_M„ ) + ^ (M^^) - M3., ^ <^33) 


21 33 

a 


31 23 


* M32 S/Ma.' - » 3 . S/Maa’ ' “aa S/Ma," 
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+ < »Z ,»33 


MM ) + ^ (^ 13 ^ 
31 22 oX, 


]] 


* 6 j “ ( ) 

= [ ^ ( « 3 > 1 /^' ^ ^ ^ 

- ^31 h ■ *^22 ^ 

{ ' 


M k (Mo,) *^32 ^ ^^ 21 ^ 

21 ax^ 32 '^*1 


(M,,) 


+ ^ ”31 ^^^^12^ + ”12 ■ ^1 3 Xj 3 ^ 


Mil ax/^ 22 ^ 

^ N 


- «3. ] + B 33 S;/r3)*r3 

+ «32 I; <”u' - » 2 , s.‘”> 2 ’ ' 

i 1 


- (B r 

31 


+ ^ 00^0 { ^ 11 ^ *^22 
1 32 2 33 3 I ^ i 


I: (M„) - M23 l;,‘”3^>' 


+ M 33 S <« 33 > - M 33 

^ a 

.( M„M„ - M33M33 ». 3 ‘ s/'’ 33 ' 


22 33 

a 


+ M_„ ^ (M^J - M,, (^23^ " ^23 ax/^31^ 


‘33 ax ' 21 ' 


31 


I- (M^,) 


(MM -MM 

' 21 33 31 23 


) + k ‘'^ 12 ) ■ ^ 3 ^ ^21 ax^ 32 


+ M 


I- (M ) - M k ^^22^ ■ ^22 Ie/^31^^ 

32 ax 21 ' 31 ax, i 


+ ( M, M,, - M, M„ ) + aJ 


*S ."^' 3 ’ ) ] 


The B matrix is defined as 


B = |^[f(x, u)] 
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0 

0 

0 


M 

M 

Mo., 

M 

22 

33 

32 

23 

“(M 

M - 

M., 

M 

^ 21 

33 

31 

23 

M 

M - 

Mo, 

M . 

21 

32 

31 

22 


0 

0 

0 


~(M,, 


M 

M ) 

12 

33 

32 

13 

M,. 

M ** 

M 

M 

11 

33 

31 

13 

-(M,, 


M 

M.o) 

1 1 

32 

31 

12 


0 

0 

0 

^12^23~ ^22^13 

M.t”22- »^21^2 


The various terms in the expressions above are expanded in terms 
of the quantities defined in the equations of motion in (B.l) 
above and are given below. 


(M.J = 0 for i 

dXj jk 


= A, 5, 6, j = 1, 2, 3 and A" = Ir 2, 3 


5 (M„) = - ">31/33^3 - n>3l,l,S^ -"1313^382 - 

2 


■ '"3'=3'l®23 


I; (M,3) = - 2ni3l3l,S, - ™3P3l,S3 - m3P3l,S33 

2 


S3'».3> =■ 


^ ) = 

0X3 11 




- “3^3^1223 “ "‘3^2P3^3 ' 




2“3'2'’3=3 - "'3'’3‘l® 


23 


® 3 ^ 3 ^ 2^3 ~ ™ 3 ^ 3 ^ 1^23 
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S '"3‘.P2®3 


It (m,,) ■ 0 

dx.^ 22 


S '« 33 ' " “ 

2 


^ (Mg,) = - 
3 

s <M22> ■ - '"aVsSs - 

3 


(Moo) = “ 

9 X^ 23 3 3 2 3 


s «3l> = - “sl/aSaa 

2 


S 3 '» 33 ' ‘ “ 


S « 33 ' = “ 


i;^(M 3 ,) - - n, 3 l,P 3 S 33 - «. 3 l 3 P 3 S 3 


S (M 33 ) = - m3l3P3S3 

3 


— (M ) = 0 

6X3^ 33' 


|^^(r,) = - ni 3 l,P 3 X^C 33 - ^^31,13X^03 

■*■ ”' 2 ^ 1 ^ 2 ^ 4^2 ^ ''" 3 ^ 2 ^ 1 ^ 45^2 '*’ ™ 2 ^ 2 ^ 1 ^ 2^45 
+ "‘ 3 P 3 ^^ 23'^456 

1^ (r,) = - m3l,P3X,C33 - m3l3P3X,,C3 
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d , . 

s '■'i' 

4 


0 / \ 


^ / s 

O 

3 / 1 

S 

2 


S 3 '=^Z' 


- '" 3 '’ 3 *E=' 456=3 + 

- 2 ("'3l,P3X,S33 + ni3l3P3X^3S3 + m33l,l3X,S3 - 

”3^3*2*466 = 3 " ”2*.V4=2 ' ”3*2*1*46 ’ ”2P2*,*45=! 
' ” 3 '’ 3 * 1*466 

- 2 <”312P3*45 = 3 " ”3P3*2*466 = 3 ' ”3*3*2*45=2 


- “2'*2*1*46 = 2 - “sPa*, *456 = 23* 


2 <- ” 3 ^ 3 * 2 * 466=3 “ ” 3 ^ 3 * 1 * 466 = 23 * 


= 0 


‘ - ” 3 *iP 3 * 4'*23 - ” 23 * 2 * 1 * 4 '*! 


Z 1 Z 4 4 :!; 


- ” 3 *lP 3 * 4'*23 *■ ” 3 * 2 ^ 3 * 45=3 


”3^3*2*456=3 


= - 2 (m3l,P32,S33 + in3l3P3X,,S3 4- - 


m, 


p 1 X S - rn l^p^x^S^) 

ra 2 456 3 3 2*^2 4 2 


^ / I 

^ 3 ** 2 * 

^ 3 ** 2 * 

l;/* 3 > 

H 3 <* 3 * 

S<* 3 > 

4 

0 , ^ 

ai^'Pa* 


- 2 I”3*2P3*4S=3 - ”3^3*2*456=3* 


2 ” 3 P 3 * 2 * 456=3 


= 0 


- >Vl« 4 ^ 3^23 


-VlP 3 '' 4^23 - 

- 2 (™3l,P3X,S23 + in3l3P3X3^gS3) 


- 2 m 3 l 2 p 3 ^ 45^3 
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where, 

represents the elements of the Inertia matrix as 
defined in (B.l), 

x^ is the f*'*' state vector as defined in (B.3), 

r^ is a function of the state vectors as defined in (B.5), 

X, = X + . . . + X , 

1 . . n 1 n 

S, = sin (x + . . . + X ), 

C = cos (x^ + . . . + X ) and 

A = det [M(q)]. 
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